

#ifndef _KINECTFUNCTION_H_
#define _KINECTFUNCTION_H_

#include <XnOpenNI.h>
#include <XnCodecIDs.h>
#include <XnCppWrapper.h>
#include <math.h>

static xn::Context g_Context;
static xn::DepthGenerator g_DepthGenerator;
static xn::UserGenerator g_UserGenerator;
static XnBool g_bNeedPose ;
static XnChar g_strPose[20] ;
static XnBool g_bDrawBackground;
static XnBool g_bDrawPixels;
static XnBool g_bDrawSkeleton;
static XnBool g_bPrintID;
static XnBool g_bPrintState;
static XnBool g_bPause;
static XnBool g_bRecord;
static XnBool g_bQuit;
static float arm1,arm2;	//arm1 is from hand to elbow, arm2 is from elbow to shoulder
#define CALCDISTANCE(j1,j2) (sqrt(pow((j1).X-(j2).X,2)+pow((j1).Y-(j2).Y,2)+pow((j1).Z-(j2).Z,2)))

static XnFloat Colors[][3] =
{
	{0,1,1},
	{0,0,1},
	{0,1,0},
	{1,1,0},
	{1,0,0},
	{1,.5,0},
	{.5,1,0},
	{0,.5,1},
	{.5,0,1},
	{1,1,.5},
	{1,1,1}
};

// Callback: New user was detected
static void XN_CALLBACK_TYPE User_NewUser(xn::UserGenerator& generator, XnUserID nId, void* pCookie)
{
	printf("New User %d\n", nId);
	
	// New user found
	if (g_bNeedPose)	//start detecting pose
	{
		g_UserGenerator.GetPoseDetectionCap().StartPoseDetection(g_strPose, nId);
	}
	else
	{
		g_UserGenerator.GetSkeletonCap().RequestCalibration(nId, TRUE);
	}
}

// Callback: An existing user was lost
static void XN_CALLBACK_TYPE User_LostUser(xn::UserGenerator& generator, XnUserID nId, void* pCookie)
{
	printf("Lost User %d\n",nId);
}
// Callback: Detected a pose
static void XN_CALLBACK_TYPE UserPose_PoseDetected(xn::PoseDetectionCapability& capability, const XnChar* strPose, XnUserID nId, void* pCookie)
{

	printf("Pose %s detected for user %d\n",strPose,nId);
	g_UserGenerator.GetPoseDetectionCap().StopPoseDetection(nId);
	g_UserGenerator.GetSkeletonCap().RequestCalibration(nId, TRUE);	/*request calibration*/
}
// Callback: Started calibration
static void XN_CALLBACK_TYPE UserCalibration_CalibrationStart(xn::SkeletonCapability& capability, XnUserID nId, void* pCookie)
{

	printf("Calibration started for user %d\n");

}
// Callback: Finished calibration
static void XN_CALLBACK_TYPE UserCalibration_CalibrationEnd(xn::SkeletonCapability& capability, XnUserID nId, XnBool bSuccess, void* pCookie)
{
	if (bSuccess)
	{
		// Calibration succeeded
		printf("Calibration complete, start tracking user %d\n", nId);
		g_UserGenerator.GetSkeletonCap().StartTracking(nId);	/*start skeleton tracking*/
		//get arm1 and arm2
		arm1=0;arm2=0;
		for(int i=0;i<10;i++) {
			XnUserID aUsers[15];
			XnUInt16 nUsers = 15;
			g_UserGenerator.GetUsers(aUsers, nUsers);

			// Setup the OpenGL viewpoint
			xn::SceneMetaData sceneMD;
			xn::DepthMetaData depthMD;
			g_DepthGenerator.GetMetaData(depthMD);

			//Updates all generators nodes in the context, waiting for all to have new data.
			g_Context.WaitAndUpdateAll();

			/*Gets the current depth-map meta data, including depth, pixel*/
			g_DepthGenerator.GetMetaData(depthMD);
			/*Get the pixels that belong to a user. */
			g_UserGenerator.GetUserPixels(0, sceneMD);

			g_UserGenerator.GetUsers(aUsers, nUsers);
			//if skeleton is trackable
			if (g_bDrawSkeleton && g_UserGenerator.GetSkeletonCap().IsTracking(aUsers[0]))
			{
				XnSkeletonJointPosition joint1, joint2, joint3;//shoulder and elbow and hand
				g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(aUsers[0], XN_SKEL_RIGHT_SHOULDER, joint1);
				g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(aUsers[0], XN_SKEL_RIGHT_ELBOW, joint2);
				g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(aUsers[0], XN_SKEL_RIGHT_HAND, joint3);
				XnVector3D rShoulder = joint1.position, rElbow = joint2.position, rHand = joint3.position;
				arm1 += CALCDISTANCE(rHand,rElbow);
				arm2 += CALCDISTANCE(rShoulder,rElbow);

				/*printf("arm1 %.2f, arm2 %.2f, \n, LEFT:  %.2f, %.2f, %.2f \t\tRIGHT: %.2f, %.2f, %.2f\n",
					arm1/(i+1),arm2/(i+1),rShoulder.X,rShoulder.Y,rShoulder.Z,
					rHand.X,rHand.Y,rHand.Z);	*/
			}
		}
		arm1/=10;//take average
		arm2/=10;
	}
	else
	{
		// Calibration failed
		printf("Calibration fail for user %d\n", nId);
		if (g_bNeedPose)
		{	/*back to pose detection*/
			g_UserGenerator.GetPoseDetectionCap().StartPoseDetection(g_strPose, nId);
		}
		else
		{
			g_UserGenerator.GetSkeletonCap().RequestCalibration(nId, TRUE);
		}
	}
}

#endif